#!/usr/bin/env python3
"""
导航控制节点
该节点负责接收导航指令，并使用ROS的move_base功能包将机器人导航到预设的位置。
它还发布导航状态，并通过TTS（文本转语音）提供语音反馈。
"""

import rospy
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from std_msgs.msg import String
import actionlib
from actionlib_msgs.msg import GoalStatus

class NavController:
    """
    导航控制类，封装了所有与机器人导航相关的功能。
    - 初始化ROS节点、发布器和订阅器。
    - 连接到move_base动作服务器。
    - 处理导航指令。
    - 管理导航状态。
    - 提供取消导航和状态报告的功能。
    """
    def __init__(self):
        """
        初始化NavController类的实例。
        """
        rospy.init_node('nav_controller')
        
        # 标志位，用于跟踪机器人是否正在进行导航
        self.is_navigating = False

        # 预设的房间位置坐标，格式为 [x, y, z, qx, qy, qz, qw]
        self.room_locations = {
            "302": [3.2856, -0.095932, 0, 0, 0.63057, 0.77614],
            "327": [0.79482, 2.2434, 0, 0, 0.94996, 0.21236]
        }
        
        # 初始化发布器
        # tts_pub: 用于发布需要语音播报的文本
        # nav_status_pub: 用于发布机器人当前的导航状态 (e.g., "idle", "active")
        self.tts_pub = rospy.Publisher('tts_text', String, queue_size=10)
        self.nav_status_pub = rospy.Publisher('navigation_status', String, queue_size=1, latch=True)
        
        # 初始化actionlib客户端，用于与move_base通信
        self.move_base = actionlib.SimpleActionClient('move_base', MoveBaseAction)
        rospy.loginfo("正在等待move_base动作服务器连接...")
        
        # 设置超时（30秒），等待move_base服务器启动
        if not self.move_base.wait_for_server(rospy.Duration(30.0)):
            rospy.logerr("无法连接到move_base服务器！请检查导航系统是否正常启动。")
            self.tts_pub.publish("导航系统连接失败，请检查系统状态。")
        else:
            rospy.loginfo("move_base服务器已成功连接。")

        # 订阅导航指令话题，接收来自其他节点的导航请求
        rospy.Subscriber('navigation_command', String, self.navigation_callback)
        
        # 发布初始状态为 "idle"（空闲）
        self.nav_status_pub.publish("idle")
        rospy.loginfo("导航控制器已启动，等待导航指令。")

    def navigation_callback(self, msg):
        """
        接收并处理导航指令的回调函数。
        
        Args:
            msg (std_msgs.msg.String): 包含目标房间名称的ROS消息。
        """
        if self.is_navigating:
            rospy.logwarn("正在导航中，请等待导航结束后再试")
            self.tts_pub.publish("我正在导航，请稍等。")
            return
            
        room = msg.data
        if room in self.room_locations:
            rospy.loginfo(f"接收到导航命令，前往 {room} 房间")
            self.navigate_to_room(room)
        else:
            rospy.logwarn(f"未知的导航目标: {room}")
            self.tts_pub.publish(f"抱歉，我不知道{room}房间的位置。")

    def navigate_to_room(self, room):
        """
        根据指定的房间名称，创建并发送导航目标到move_base。
        
        Args:
            room (str): 目标房间的名称，必须在self.room_locations中存在。
        """
        self.is_navigating = True
        
        # 更新导航状态为 "active"（活动中）
        self.nav_status_pub.publish("active")
        
        # 创建导航目标（MoveBaseGoal）
        goal = MoveBaseGoal()
        goal.target_pose.header.frame_id = "map"
        goal.target_pose.header.stamp = rospy.Time.now()
        
        # 从字典中获取预设的位置和姿态信息
        location = self.room_locations[room]
        goal.target_pose.pose.position.x = location[0]
        goal.target_pose.pose.position.y = location[1]
        goal.target_pose.pose.orientation.x = location[2]
        goal.target_pose.pose.orientation.y = location[3]
        goal.target_pose.pose.orientation.z = location[4]
        goal.target_pose.pose.orientation.w = location[5]
        
        # 发送导航目标，并指定完成时的回调函数
        self.move_base.send_goal(goal, done_cb=self.navigation_done_callback)
        rospy.loginfo(f"正在导航到 {room} 房间")
        self.tts_pub.publish(f"出发，前往{room}。")

    def navigation_done_callback(self, status, result):
        """
        导航任务完成时的回调函数。
        
        Args:
            status (int): actionlib的最终状态码。
            result (MoveBaseResult): 导航任务的结果。
        """
        self.is_navigating = False
        self.nav_status_pub.publish("idle")
        
        rospy.loginfo(f"导航完成，状态码: {status} ({GoalStatus.to_string(status)})")

        # 根据不同的状态码，生成不同的语音反馈信息
        if status == GoalStatus.SUCCEEDED:
            final_message = "已成功到达目的地。现在您可以下达新的指令。"
        elif status == GoalStatus.PREEMPTED:
            final_message = "导航任务已被新的指令取消。现在您可以下达新的指令。"
        elif status == GoalStatus.ABORTED:
            final_message = "导航失败，可能是因为路径被完全阻塞，或者我暂时找不到路。请您寻求他人帮助。"
        elif status == GoalStatus.REJECTED:
            final_message = "导航目标无法到达，它可能在障碍物内部或地图范围之外。"
        else:   
            final_message = f"导航因未知原因失败，状态为 {GoalStatus.to_string(status)}。"

        rospy.loginfo(final_message)
        self.tts_pub.publish(final_message)

    def cancel_navigation(self):
        """
        取消当前的导航任务。
        如果当前有导航任务在执行，则向move_base发送取消请求。
        """
        if self.is_navigating:
            self.move_base.cancel_goal()
            rospy.loginfo("导航已被用户取消")
            self.tts_pub.publish("导航已取消")
        else:
            self.tts_pub.publish("当前没有进行中的导航任务")

    def report_navigation_status(self):
        """
        报告机器人当前的导航状态。
        """
        if self.is_navigating:
            state = self.move_base.get_state()
            # 根据move_base客户端的状态提供更具体的反馈
            if state == GoalStatus.ACTIVE:
                status_msg = "正在执行导航任务"
            elif state == GoalStatus.PENDING:
                status_msg = "导航任务已接收，等待执行"
            else:
                status_msg = f"导航状态：{GoalStatus.to_string(state)}"
        else:
            status_msg = "当前空闲，可以接收新的导航指令"
        
        # 附加上可用的导航点信息
        available_rooms = ", ".join(self.room_locations.keys())
        full_status = f"{status_msg}。可用房间：{available_rooms}"
        rospy.loginfo(full_status)
        self.tts_pub.publish(full_status)

if __name__ == '__main__':
    try:
        nav_controller = NavController()
        # 保持节点运行，等待回调函数被触发
        rospy.spin()
    except rospy.ROSInterruptException:
        rospy.loginfo("导航控制器节点关闭")
